/*************对象从属关系与命名规则***********************
 @object:R2
   @object:Gantry   龙门架
     @motor:P       向上动角度变大的电机
     @motor:N       向上动角度变小的电机

   @object:Scara    机械臂
     @motor:Main    大臂电机
     @motor:End     小臂电机

   @object:Comm     末端执行机构
     @motor:Actor   吸盘自转电机
     @switch:Punk   吸盘真空泵
     @switch:Swiv   吸盘泄气电磁阀

   @object:Chassis  底盘
*********************************************************/

/* ==============头文件=====================================*/
#include "Up_Ctrl.h"
#include "RCS_Stastic.h"
#include "Valve_2006.h"
#include "rcs.h"
#include "RCS_MOTOR.h"


/* ==============全局变量===================================*/
#define RM3508_Test_CAN_GROUP		CAN_GROUP_1 	// 默认挂在CAN1上
#define RM3508_Test_CAN_ID			5			        // 电机ID
#define RM2006_Test_CAN_GROUP		CAN_GROUP_1         // 2006挂在CAN1上
#define RM2006_Test_CAN_ID			1                   // 2006电机ID

Motor_Ctrl_Node RM3508_Test; // 结点定义
Motor_Ctrl_Node RM2006_Test; // 2006结点定义
Motor_Ctrl_Node Shoot1_Test; //3508发射测试结点
Motor_Ctrl_Node Shoot2_Test; //3508发射测试结点

PID_Struct RM3508_Test_Spd_Pid;   // 3508PID_Speed控制
PID_Struct RM3508_Test_Angle_Pid; // 3508PID_Angle控制
PID_Struct RM2006_Test_Spd_Pid;   // 2006PiD_Speed控制
PID_Struct Shoot1_Test_Spd_Pid;		//3508发射测试结点pid
PID_Struct Shoot2_Test_Spd_Pid;		//3508发射测试结点pid
PID_Struct Launch_Feed_Sync_1_Spd_Pid;//2006播弹1pid
PID_Struct Launch_Feed_Sync_2_Spd_Pid;//2006播弹2pid
PID_Struct Launch_Shoot_Mirror_1_Spd_Pid;//3508摩擦轮1pid
PID_Struct Launch_Shoot_Mirror_2_Spd_Pid;//3508摩擦轮2pid

float Demo_TestAngle_CAN1[8];
float Demo_TestAngle_CAN2[8];


/**/


Motor_Ctrl_Node Launch_Yuntai;//发射云台电机
Motor_Ctrl_Node Launch_Angle_Sync_1,Launch_Angle_Sync_2;//发射俯仰电机，保持同步
Motor_Ctrl_Node Launch_Shoot_Mirror_1,Launch_Shoot_Mirror_2;//摩擦发射点击，保持镜像
Motor_Ctrl_Node Launch_Feed_Sync_1,Launch_Feed_Sync_2;//供弹点击，保持同步



void MotorNode_Shoot_Init(){
	MotorNode_Init_C610(CAN_GROUP_2,1,&Launch_Yuntai);
	//TODO:角度的点击对应电调询问
	//MotorNode_Init_C6(CAN_GROUP_2,2,&Launch_Angle_Sync_1);
	//MotorNode_Init_C6(CAN_GROUP_2,3,&Launch_Angle_Sync_2);
	RM3508_Test_Spd_Pid = PID_Get_RM3508_Speed_Pid();							  // 获取速度环PID
	RM3508_Test_Angle_Pid = PID_Get_RM3508_Angle_Pid();             // 获取角度环PID
	MotorNode_Init_C610(CAN_GROUP_2,4,&Launch_Shoot_Mirror_1);
	MotorNode_Add_SpeedPid(&Launch_Shoot_Mirror_1, &RM3508_Test_Spd_Pid);     // 添加速度环PID
	MotorNode_Add_AnglePid(&Launch_Shoot_Mirror_1, &RM3508_Test_Angle_Pid);   // 添加角度环PID
	MotorNode_Add(CAN_GROUP_2, &Launch_Shoot_Mirror_1);             // 电机结点加入CAN总线
	MotorNode_Init_C610(CAN_GROUP_2,5,&Launch_Shoot_Mirror_2);
	MotorNode_Add_SpeedPid(&Launch_Shoot_Mirror_2, &RM3508_Test_Spd_Pid);     // 添加速度环PID
	MotorNode_Add_AnglePid(&Launch_Shoot_Mirror_2, &RM3508_Test_Angle_Pid);   // 添加角度环PID
	MotorNode_Add(CAN_GROUP_2, &Launch_Shoot_Mirror_2);             // 电机结点加入CAN总线
	
	MotorNode_Init_C620(CAN_GROUP_2,6,&Launch_Feed_Sync_1);
	MotorNode_Init_C620(CAN_GROUP_2,7,&Launch_Feed_Sync_2);
}

/*********************************************************************************

                @addtogroup:全功能接口

*********************************************************************************/
/**
 * @name:RM3508_Test_Init
 * @brief:测试RM3508电机时进行的初始化
 **/
void RM3508_Test_Init(void)
{
	RMMotor_CAN_Init(CAN1);
	RMMotor_CAN_Init(CAN2);// CAN总线初始化
	RM3508_Test.Motor_Type = 0;//3508=0，2006=1，6020=2                                                   
	MotorNode_Init_C620(RM3508_Test_CAN_GROUP, RM3508_Test_CAN_ID, &RM3508_Test); // 电调电机初始化
	PID_Init();																	  // PID初始化
	RM3508_Test_Spd_Pid = PID_Get_RM3508_Speed_Pid();							  // 获取速度环PID
	RM3508_Test_Angle_Pid = PID_Get_RM3508_Angle_Pid();             // 获取角度环PID
	MotorNode_Add_SpeedPid(&RM3508_Test, &RM3508_Test_Spd_Pid);     // 添加速度环PID
	MotorNode_Add_AnglePid(&RM3508_Test, &RM3508_Test_Angle_Pid);   // 添加角度环PID
	MotorNode_Add(RM3508_Test_CAN_GROUP, &RM3508_Test);             // 电机结点加入CAN总线
}

/**
 *@name: Demo_MotorTest_Init
 *@brief:将该函数放入MainTask_AllInit函数中的“输出部分初始化”，即可完成CAN总线上大疆电机和本杰明电调的初始化
 *@usage:不建议在新编写的代码中采用老式写法，这里放老写法只是为了方便理解一些老代码，也因为老代码方便用于测试电机
 **/
void Demo_MotorTest_Init_Old()
{
    // 大疆电机在老代码中被称为Motor
    // Motor_Init的含义是，完成大疆电机在CAN1的初始化，Motor_Init2则是CAN2
    Motor_Init();
    Motor_Init2(); // RCS_CAN.c有bug，必须先初始化CAN1再初始化CAN2
}

/**
 *@name: Demo_RMESC_Test_Old
 *@brief:老代码测试大疆电机

 *@usage:请先将UpCtrl_Task中的whil(1)循环中的“下发机器人控制指令”里面的两个MotorList_Excute注释，然后放入本函数。
 *@usage:这么做的原因是， MotorList_Excute函数底层会发送CAN报文，这会干涉到本函数的运行

 *@tips:不建议在新编写的代码中采用老式写法，这里放老写法只是为了方便理解一些老代码，也因为老代码方便用于测试电机
 *@tips:老代码有个优点，能一下子看出往CAN总线上发了几条报文，比如本函数，有三个函数和CAN1有关，一次调用就会往CAN1上发三条报文
**/
void Demo_RMESC_Test_Old()
{
    // 测试CAN1上的3508或2006
    Motor_Send(800, 800, 800, 800);
    Motor_Send_ADD(800, 800, 800, 800); // 3508和2006使用同样的报文，地位相同，控制方式和反馈方式也是一样的，只有减速比不同
    Demo_TestAngle_CAN1[0] = Get_Motor_Float_Angle(1);
    Demo_TestAngle_CAN1[1] = Get_Motor_Float_Angle(2);
    Demo_TestAngle_CAN1[2] = Get_Motor_Float_Angle(3);
    Demo_TestAngle_CAN1[3] = Get_Motor_Float_Angle(4);
    Demo_TestAngle_CAN1[4] = Get_Motor_Float_Angle(5);
    Demo_TestAngle_CAN1[5] = Get_Motor_Float_Angle(6);
    Demo_TestAngle_CAN1[6] = Get_Motor_Float_Angle(7);
    Demo_TestAngle_CAN1[7] = Get_Motor_Float_Angle(8);
    // 测试CAN1上ID为5~7的6020
//    GM6020_Send_ADD(700, 700, 700); // 6020和3508报文不同，如果CAN总线上存在ID小于5的6020，请考虑是否会出现报文干涉，判断方式可以跳转到GM6020_Send_ADD函数中，里面的注释有写怎么看00
    // 测试CAN2上的3508或2006
//    Motor_Send2(400, 400, 400, 400);
//    Motor_Send2_ADD(400, 400, 400, 400);
//    Demo_TestAngle_CAN2[0] = Get_Motor_Float_Angle2(1);
//    Demo_TestAngle_CAN2[1] = Get_Motor_Float_Angle2(2);
//    Demo_TestAngle_CAN2[2] = Get_Motor_Float_Angle2(3);
//    Demo_TestAngle_CAN2[3] = Get_Motor_Float_Angle2(4);
//    Demo_TestAngle_CAN2[4] = Get_Motor_Float_Angle2(5);
//    Demo_TestAngle_CAN2[5] = Get_Motor_Float_Angle2(6);
//    Demo_TestAngle_CAN2[6] = Get_Motor_Float_Angle2(7);
//    Demo_TestAngle_CAN2[7] = Get_Motor_Float_Angle2(8);
//    // 测试CAN1上ID为5~7的6020
//    GM6020_Send_ADD(700, 700, 700);
}

/**
 * @name:RM2006_Test_Init
 * @brief:测试RM2006电机时进行的初始化
 **/
void RM2006_Test_Init(void)
{
    RMMotor_CAN_Init(CAN1);
    RMMotor_CAN_Init(CAN2);                                                       // CAN总线初始化
    MotorNode_Init_C610(RM2006_Test_CAN_GROUP, RM2006_Test_CAN_ID, &RM2006_Test); // 610电调初始化
    PID_Init();                                                                   // PID初始化
    RM2006_Test_Spd_Pid = PID_Get_RM2006_Speed_Pid();                           // 获取速度环PID
    MotorNode_Add_SpeedPid(&RM2006_Test, &RM2006_Test_Spd_Pid);                   // 添加速度环PID
    MotorNode_Add(RM2006_Test_CAN_GROUP, &RM2006_Test);                           // 电机结点加入CAN总线
}

/**
 * @name:RM3508_Shoot_Test_Init
 * @brief:测试RM3508电机驱动发射装置进行的初始化
 **/
void RM3508_Shoot_Test_Init(void)
{
	RMMotor_CAN_Init(CAN1);
	RMMotor_CAN_Init(CAN2);// CAN总线初始化                                                  
	MotorNode_Init_C620(CAN_GROUP_2, 3, &Shoot1_Test); // 电调电机初始化
	MotorNode_Init_C620(CAN_GROUP_2, 4, &Shoot2_Test); // 电调电机初始化
	PID_Init();																	  // PID初始化
	Shoot1_Test_Spd_Pid = PID_Get_RM3508_Speed_Pid();							  // 获取速度环PID
	Shoot2_Test_Spd_Pid = PID_Get_RM3508_Speed_Pid();             // 获取角度环PID
	MotorNode_Add_SpeedPid(&Shoot1_Test, &Shoot1_Test_Spd_Pid);     // 添加速度环PID
	MotorNode_Add_SpeedPid(&Shoot2_Test, &Shoot2_Test_Spd_Pid);
	MotorNode_Add(CAN_GROUP_2, &Shoot1_Test);             // 电机结点加入CAN总线
	MotorNode_Add(CAN_GROUP_2, &Shoot2_Test);             // 电机结点加入CAN总线
}

/**
 * @name:Feed_Sync_Test_Init
 * @brief:测试拨弹盘供弹初始化函数
 **/
void Feed_Sync_Test_Init(void)
{
	RMMotor_CAN_Init(CAN1);
	RMMotor_CAN_Init(CAN2);// CAN总线初始化                                                  
	MotorNode_Init_C610(CAN_GROUP_2, 6, &Launch_Feed_Sync_1); // 电调电机初始化
	MotorNode_Init_C610(CAN_GROUP_2, 7, &Launch_Feed_Sync_2); // 电调电机初始化
	PID_Init();																	  // PID初始化
	Launch_Feed_Sync_1_Spd_Pid = PID_Get_RM2006_Speed_Pid();							  // 获取速度环PID
	Launch_Feed_Sync_2_Spd_Pid = PID_Get_RM2006_Speed_Pid();             // 获取角度环PID
	MotorNode_Add_SpeedPid(&Launch_Feed_Sync_1, &Launch_Feed_Sync_1_Spd_Pid);     // 添加速度环PID
	MotorNode_Add_SpeedPid(&Launch_Feed_Sync_2, &Launch_Feed_Sync_2_Spd_Pid);
	MotorNode_Add(CAN_GROUP_2, &Launch_Feed_Sync_1);             // 电机结点加入CAN总线
	MotorNode_Add(CAN_GROUP_2, &Launch_Feed_Sync_2);             // 电机结点加入CAN总线
}

/**
 * @name:Launch_Shoot_Init
 * @brief:云台发射装置初始化函数
 **/
void Launch_Shoot_Init(void)
{
	RMMotor_CAN_Init(CAN1);
	RMMotor_CAN_Init(CAN2);// CAN总线初始化 
	MotorNode_Init_C620(CAN_GROUP_2, 3, &Launch_Shoot_Mirror_1); // 3508电调电机初始化
	MotorNode_Init_C620(CAN_GROUP_2, 4, &Launch_Shoot_Mirror_2); // 3508电调电机初始化
	MotorNode_Init_C610(CAN_GROUP_2, 6, &Launch_Feed_Sync_1); // 2006电调电机初始化
	MotorNode_Init_C610(CAN_GROUP_2, 7, &Launch_Feed_Sync_2); // 2006电调电机初始化
	PID_Init();
	Launch_Shoot_Mirror_1_Spd_Pid = PID_Get_RM3508_Speed_Pid();							  // 获取速度环PID
	Launch_Shoot_Mirror_2_Spd_Pid = PID_Get_RM3508_Speed_Pid();
	Launch_Feed_Sync_1_Spd_Pid = PID_Get_RM2006_Speed_Pid();							  // 获取速度环PID
	Launch_Feed_Sync_2_Spd_Pid = PID_Get_RM2006_Speed_Pid();
	MotorNode_Add_SpeedPid(&Launch_Shoot_Mirror_1, &Launch_Shoot_Mirror_1_Spd_Pid);     // 添加速度环PID
	MotorNode_Add_SpeedPid(&Launch_Shoot_Mirror_2, &Launch_Shoot_Mirror_2_Spd_Pid);
	MotorNode_Add_SpeedPid(&Launch_Feed_Sync_1, &Launch_Feed_Sync_1_Spd_Pid);     // 添加速度环PID
	MotorNode_Add_SpeedPid(&Launch_Feed_Sync_2, &Launch_Feed_Sync_2_Spd_Pid);
	MotorNode_Add(CAN_GROUP_2, &Launch_Shoot_Mirror_1);             // 电机结点加入CAN总线
	MotorNode_Add(CAN_GROUP_2, &Launch_Shoot_Mirror_2);             // 电机结点加入CAN总线
	MotorNode_Add(CAN_GROUP_2, &Launch_Feed_Sync_1);             // 电机结点加入CAN总线
	MotorNode_Add(CAN_GROUP_2, &Launch_Feed_Sync_2);             // 电机结点加入CAN总线
}